#if !defined(glib_g2_matrix_h)
#define glib_g2_matrix_h

#include "vector.h"
#include "smatrix.h"
#include <algorithm>

namespace g2 {

template <typename T>
class basic_matrix;

typedef basic_matrix<double> matrix;
typedef basic_matrix<float> matrixf;
typedef basic_matrix<long double> matrixl;

template <typename T>
class basic_matrix {
public:
	union {
		struct { T xx, xy, yx, yy; };
		T e[2][2];
	};
public:
	typedef T * iterator;
	typedef const T * const_iterator;
public:
	T * begin()
		{ return &xx; }
	const T * begin() const
		{ return &xx; }
	T * end()
		{ return &yy + 1; }
	const T * end() const
		{ return &yy + 1; }
	static int size() 
		{ return 4; }
public:
	basic_matrix()
		{ }
	basic_matrix(T _xx, T _xy, T _yx, T _yy) :
		xx(_xx), xy(_xy), yx(_yx), yy(_yy)
		{ }
	template <class U> 
	basic_matrix(const basic_matrix<U> & v)  :
		xx(T(v.xx)), xy(T(v.xy)), yx(T(v.yx)), yy(T(v.yy)) { }
	template <class U> 
	basic_matrix(const basic_smatrix<U> & v)  :
		xx(T(v.xx)), xy(T(v.xy)), yx(T(v.xy)), yy(T(v.yy)) { }
	basic_matrix & identity(T diagval = 1) 
		{ xx = yy = diagval; xy = yx = 0; return * this; }
	basic_matrix & zero() 
		{ xx = yy = xy = yx = 0; return * this; }
public:
	const T & operator ()(int i, int j) const
		{ return e[i][j]; }
	T & operator ()(int i, int j)
		{ return e[i][j]; }
	basic_vector<T> operator ()(const basic_vector<T> & v) const 
		{ return basic_vector<T>(xx * v.x + xy * v.y, yx * v.x + yy * v.y); }
public:
	basic_vector<T> col(int i) const
		{ return basic_vector<T>(e[0][i], e[1][i]); }
	basic_matrix<T> & set_col(int i, const basic_vector<T> & v)
		{ e[0][i] = v.x; e[1][i] = v.y; return * this; }
	basic_vector<T> row(int i) const
		{ return basic_vector<T>(e[i][0], e[i][1]); }
	basic_matrix<T> & set_row(int i, const basic_vector<T> & v)
		{ e[i][0] = v.x; e[i][1] = v.y; return * this; }
	T det() const
		{ return xx*yy - xy*yx; };
	operator bool() const ///< true if not degenerate
		{ return det() != 0; };
	basic_matrix<T> & transpose(); ///< transposes itself
	basic_matrix<T> transposed() const; ///< returns transposed matrix, while leaves itself intact
	basic_matrix<T> inverse() const;
	basic_vector<T> inverse(const basic_vector<T> & v) const; ///< \return product of inverse matrix and given vector
	basic_smatrix<T> ssquare() const; ///< product of (*this) and (*this)^T
public:
	basic_matrix & operator += (const basic_matrix<T> & v)
		{ xx += v.xx; xy += v.xy; yx += v.yx; yy += v.yy; return * this; }
    basic_matrix & operator += (T a) ///< adds identity matrix multiplied on a
		{ xx += a; yy += a; return * this; }
    basic_matrix & operator -= (const basic_matrix<T> & v)
		{ xx -= v.xx; xy -= v.xy; yx -= v.yx; yy -= v.yy; return * this; }
    basic_matrix & operator -= (T a) ///< subtracts identity matrix multiplied on a
		{ xx -= a; yy -= a; return * this; }
    basic_matrix & operator *= (T a)
		{ xx *= a; xy *= a; yx *= a; yy *= a; return * this; }
    basic_matrix & operator /= (T a)
		{ xx /= a; xy /= a; yx /= a; yy /= a; return * this; }
	bool operator == (const basic_matrix<T> & v) const
		{ return xx == v.xx && xy == v.xy && yx == v.yx && yy == v.yy; }
	bool operator != (const basic_matrix<T> & v) const
		{ return !(*this == v); }
};

//inlines

template <typename T>
inline basic_matrix<T> & basic_matrix<T>::transpose()
{
	std::swap(xy, yx);
	return * this;
}

template <typename T>
inline basic_matrix<T> basic_matrix<T>::transposed() const
{
	return basic_matrix<T>(*this).transpose();
}

template <typename T>
inline basic_matrix<T> basic_matrix<T>::inverse() const {
	return basic_matrix<T>(yy, -xy, -yx, xx) / det();
}

template <typename T>
inline basic_vector<T> basic_matrix<T>::inverse(const basic_vector<T> & v) const {
	return basic_vector<T>(
		yy * v.x - xy * v.y,
		-yx * v.x + xx * v.y
	) / det();
}

template <typename T>
inline basic_smatrix<T> basic_matrix<T>::ssquare() const {
	return basic_smatrix<T>(
		xx * xx + xy * xy,
		xx * yx + xy * yy,
		yx * yx + yy * yy
	);
}

template <typename T>
inline basic_matrix<T> operator + (const basic_matrix<T> & a, const basic_matrix<T> & b) {
	basic_matrix<T> t(a);
    return t += b;
}

template <typename T>
inline basic_matrix<T> operator + (const basic_matrix<T> & v, T a) {
	basic_matrix<T> t(v);
    return t += a;
}

template <typename T>
inline basic_matrix<T> operator - (const basic_matrix<T> & a, const basic_matrix<T> & b) {
	basic_matrix<T> t(a);
    return t -= b;
}

template <typename T>
inline basic_matrix<T> operator - (const basic_matrix<T> & v, T a) {
	basic_matrix<T> t(v);
    return t -= a;
}

//unary minus
template <class T>
inline basic_matrix<T> operator - (const basic_matrix<T> & a) {
	return basic_matrix<T>(-a.xx, -a.xy, -a.yx, -a.yy);
}

template <typename T>
inline basic_matrix<T> operator * (const basic_matrix<T> & v, T a) {
	basic_matrix<T> t(v);
    return t *= a;
}

template <typename T>
inline basic_matrix<T> operator * (T a, const basic_matrix<T> & v) {
	basic_matrix<T> t(v);
    return t *= a;
}

/// returns m * v
template <typename T>
inline basic_vector<T> operator * (const basic_matrix<T> & m, const basic_vector<T> & v) {
    return m(v);
}

/// returns m^T * v = (v * m)^T
template <typename T>
inline basic_vector<T> operator * (const basic_vector<T> & v, const basic_matrix<T> & m) {
    return basic_vector<T>(m.xx * v.x + m.yx * v.y, m.xy * v.x + m.yy * v.y);
}

template <typename T>
inline basic_matrix<T> operator * (const basic_matrix<T> & a, const basic_matrix<T> & b) {
    return basic_matrix<T> (
		a.xx * b.xx + a.xy * b.yx,
		a.xx * b.xy + a.xy * b.yy,
		a.yx * b.xx + a.yy * b.yx,
		a.yx * b.xy + a.yy * b.yy
	);
}

template <typename T>
inline basic_matrix<T> operator * (const basic_smatrix<T> & a, const basic_matrix<T> & b) {
    return basic_matrix<T> (
		a.xx * b.xx + a.xy * b.yx,
		a.xx * b.xy + a.xy * b.yy,
		a.xy * b.xx + a.yy * b.yx,
		a.xy * b.xy + a.yy * b.yy
	);
}

template <typename T>
inline basic_matrix<T> operator * (const basic_matrix<T> & a, const basic_smatrix<T> & b) {
    return basic_matrix<T> (
		a.xx * b.xx + a.xy * b.xy,
		a.xx * b.xy + a.xy * b.yy,
		a.yx * b.xx + a.yy * b.xy,
		a.yx * b.xy + a.yy * b.yy
	);
}
	
//in general (non commutative matrices a & b)
template <typename T>
inline basic_matrix<T> operator * (const basic_smatrix<T> & a, const basic_smatrix<T> & b) {
    return basic_matrix<T> (
		a.xx * b.xx + a.xy * b.xy,
		a.xx * b.xy + a.xy * b.yy,
		a.xy * b.xx + a.yy * b.xy,
		a.xy * b.xy + a.yy * b.yy
	);
}

template <typename T>
inline basic_matrix<T> operator / (const basic_matrix<T> & v, T a) {
	basic_matrix<T> t(v);
    return t /= a;
}

/// tensor product: a * b^T
template <class T>
inline basic_matrix<T> tproduct(const basic_vector<T> & a, const basic_vector<T> & b) {
	return basic_matrix<T>(
		a.x * b.x, //xx
		a.x * b.y, //xy
		a.y * b.x, //yx
		a.y * b.y  //yy
	);
}

/// returns s^T * e * s 
template <class T>
inline basic_smatrix<T> duplex_product(const basic_smatrix<T> & e, const basic_matrix<T> & s) 
{
	return basic_smatrix<T>(s.transposed() * basic_matrix<T>(e) * s);
	//too ineffective
	//T sxxxx = s.xx * s.xx;
	//T sxxxy = s.xx * s.xy;
	//T sxxyx = s.xx * s.yx;
	//T sxxyy = s.xx * s.yy;
	//T sxyxy = s.xy * s.xy;
	//T sxyyx = s.xy * s.yx;
	//T sxyyy = s.xy * s.yy;
	//T syxyx = s.yx * s.yx;
	//T syxyy = s.yx * s.yy;
	//T syyyy = s.yy * s.yy;
	//return basic_smatrix<T>(
	//	sxxxx * e.xx + 2 * sxxyx * e.xy + syxyx * e.yy, //xx
	//	sxxxy * e.xx + (sxyyx + sxxyy) * e.xy + syxyy * e.yy, //xy
	//	sxyxy * e.xx + 2 * sxyyy * e.xy + syyyy * e.yy //yy
	//);
}

} //namespace g2

#endif // glib_g2_matrix_h
